#include <rtthread.h>

#include <stdio.h>
#include <io/rttSerial.h>
#include <fport.h>
#include <copter.h>
#include <rc.h>

using namespace NXMFC;

#ifndef FPORT_DEVNAME
#define FPORT_DEVNAME ":dev/uart4"
#endif

static void rcThread(void *p)
{
    rttSerial ser;
    fport fp;
    rc r;

    if (!ser.open(FPORT_DEVNAME))
    {
        printf("open fail\n");
        return;
    }

    if (!fp.allocBuf())
    {
        printf("no mem\n");
        return;
    }

    ser.setBaudRate(115200);
    ser.setHalfDuplex(true);

    while (1)
    {
        fport::RecvResult rr;
        int type;

        rr = fp.recv(ser, type, 1000);
        if (rr == fport::FPRR_DONE)
        {
            if (type == 0)
            {
                uint16_t rol, pit, thr, yaw;
                uint32_t flags;

                fp.getChannel(0, rol);
                fp.getChannel(1, pit);
                fp.getChannel(2, thr);
                fp.getChannel(3, yaw);

                flags = r.checkStick(rol, pit, thr, yaw);
                if (gCopter.armed())
                {
			        if (r.isLeftStickBottomLeft(flags))
			        {
				        gCopter.setArmed(false); //上锁
			        }

                    gCopter.setThrottle((thr - 990)/1000.0f);
                }
                else
                {
                    if (r.isLeftStickBottomRight(flags) && r.isRightStickBottomLeft(flags))
                    {
                        gCopter.setArmed();
                    }
                }
            }
        }
    }
}

int taskRcInit(void)
{
    rt_thread_t t;
    int ret = -1;

    t = rt_thread_create("rc", rcThread, 0,
                         1024, 20, 5);
    if (t)
        ret = rt_thread_startup(t);

    return ret;
}
INIT_APP_EXPORT(taskRcInit);
